#include <ros/ros.h>
#include <serial/serial.h>
#include <std_msgs/String.h>
#include <termios.h>
#include <string>
#include <geometry_msgs/Twist.h>
#include <std_msgs/Float64.h>

serial::Serial port;
std::string s;
int getch()
{ 
  static struct termios oldt, newt;
  tcgetattr( STDIN_FILENO, &oldt);           // save old settings
  newt = oldt; 
  newt.c_lflag &= ~(ICANON);                 // disable buffering      
  newt.c_cc[VMIN] = 0; newt.c_cc[VTIME] = 0;
  tcsetattr( STDIN_FILENO, TCSANOW, &newt);  // apply new settings
   
  int c = getchar();  // read character (non-blocking)
  
  tcsetattr( STDIN_FILENO, TCSANOW, &oldt);  // restore old settings
  return c;
 }

std::string getString(double a, double b){
    //a 旋转0-0.5， b 前进
    std::string cur="!M ";
    cur += std::to_string(int(a*100));
    cur += " ";
    cur += std::to_string(int(b*200));
    cur += "\r";
    return cur;
}

void controllerCallback(const geometry_msgs::Twist::ConstPtr &msg){
    s = getString(msg->angular.z, msg->linear.x);
    printf("get msg: %2.2f, %2.2f\n", msg->angular.z, msg->linear.x);
    if(port.isOpen()){
        printf("pub: ");
        printf(s.c_str());
        printf("\n");
        // port.write(s.c_str());
    }
    return;
}

int main(int arg, char **argv){
    ros::init(arg, argv, "track_controller_node");
    ros::NodeHandle nh;
    ros::Duration(5).sleep();

    ros::Subscriber cmd_sub = nh.subscribe("/smb_velocity_controller/cmd_vel", 10, controllerCallback);

    port.setPort("/dev/trackcontroller");
    port.setBaudrate(115200);
    serial::Timeout to = serial::Timeout::simpleTimeout(1000);
    port.setTimeout(to);

    s = getString(0.0,0.0);
    while(ros::ok()){
        int c;
        c = getch();

        if(c=='o'){
            if(port.isOpen()){
                std::cout<<"Port already opened."<<std::endl;
                continue;
            }
            try{
                port.open();
                std::cout<<"Port opened."<<std::endl;
            }
            catch(serial::IOException& e){
                ROS_ERROR_STREAM("Open Port Fail.");
            }
        }else if(c=='c'){
            if(port.isOpen()){
                port.close();
                std::cout<<"Port closed."<<std::endl;
            }else{
                std::cout<<"Not open?"<<std::endl;
            }
        }
        ros::spinOnce();
        // }else if(c=='f'){
        //     ROS_INFO("Forward");
        //     std::string s = "!M 0 -100\r";
        //     port.write(s.c_str());
        // }else if(c=='l'){
        //     ROS_INFO("Left");
        //     std::string s = "!M 50 0\r";
        //     port.write(s.c_str());
        // }else if(c=='r'){
        //     ROS_INFO("Right");
        //     std::string s = "!M -50 0\r";
        //     port.write(s.c_str());
        // }else if(c=='b'){
        //     ROS_INFO("Back");
        //     std::string s = "!M 0 100\r";
        //     port.write(s.c_str());
        // }else if(c=='s'){
        //     ROS_INFO("Stop");
        //     std::string s = "!M 0 0\r";
        //     port.write(s.c_str());
        // }
    }
    return 0;
}